/*
 * main.c
 */
#include "System\F2806x_Device.h"
#include <stdio.h>
#include <string.h>
#include "Drivers\debugLED.h"
#include "Timer.h"
#include "CANDriver.h"
#include "Datafile.h"
#include "Drivers\UARTDriverDBG.h"
#include "Drivers\UARTDriverB.h"
#include "Controller\Ctrl.h"

#include "CommStuff\UR_GOTO.h"
#include "CommStuff\UR_LIGHTS.h"
#include "CommStuff\UR_MANUAL.h"
#include "CommStuff\US_DATA.h"
#include "CommStuff\US_STATUS.h"

extern "C" void InitSysCtrl(void);

// flash support
#define FLASH 1
extern "C" void InitFlash(void);
extern "C" void MemCopy(Uint16 *SourceAddr, Uint16* SourceEndAddr, Uint16* DestAddr);
extern Uint16 RamfuncsLoadStart, RamfuncsLoadEnd, RamfuncsRunStart;

#define IO_BASE 	0x010000
#define SERVO_BASE 	0x020000
#define IMU_BASE 	0x030000
#define CAN_BASE 	0x040000

// Drivers
DebugLED ledDebug;

// Components
Timer timer;
Timer timerINT;
CANDriver canDrv;
SDataFile df;
UARTDriverB uartB;
UARTDriverDBG uartDBG;
URManual urManual;
URLights urLights;
USStatus usStatus;
CtrlModelClass ctrlMdl;

// main step time
#define STEPTIME 0.01 /* standard steptime, 100 Hz */

bool GetNextSentence(char* buffer, int count);
char g_GlobalSerBuffer[200+1];
int g_GlobalSerIndex = 0;

int main(void)
{
	//=================================
	//	INITIALISATION - General
	//=================================
	InitSysCtrl();	// Device Life support & GPIO mux settings

	// init debug LED
	ledDebug.Init();

	// init Serial
	uartDBG.Init(57600);
	uartDBG.SendData("XBeeBoard v1.5 - R10 Edition\n\r");

// Only used if running from FLASH
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The  RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
	MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart);

// Call Flash Initialization to setup flash waitstates
// This function must reside in RAM
	InitFlash();	// Call the flash wrapper init function
#endif //(FLASH)

	// Init Components
	timer.Init();
	timerINT.Init();
	canDrv.Init();
	uartB.Init(57600);
	ctrlMdl.initialize();

	// Reset DataFile
	memset(&df, 0, sizeof(df));
	memset(&g_GlobalSerBuffer, 0, sizeof(g_GlobalSerBuffer));

	// Enable global Interrupts and higher priority real-time debug events:
	EINT;   // Enable Global interrupt INTM
	ERTM;	// Enable Global realtime interrupt DBGM

//=================================
//	Endless LOOP
//=================================
	for(;;)  //infinite loop
	{
		timer.BeginPeriod();
		df.Counter++;

		// Process UART Inputs/Commands
		//char buffer[200];
		//Uint32 count = uartB.GetData(buffer);
		//bool executed = GetNextSentence(buffer, count);

		// Receive CAN Inputs/Commands
		float received;
		canDrv.ReadCANMBOX(0, &df.BattVoltage);
		if( canDrv.ReadCANMBOX(1, &received) ) df.dYaw = -received; // invert
		canDrv.ReadCANMBOX(2, &df.MagY); // MagY
		canDrv.ReadCANMBOX(3, &df.MagZ); // MagZ
		if( canDrv.ReadCANMBOX(10, &received) ) df.Mode = (Uint32)received;
		if( canDrv.ReadCANMBOX(11, &received))
		{
			if(received == 1)
			{
				df.AckError = true; // must be autocleared!
				df.Mode = 0;
			}
		}
		if( canDrv.ReadCANMBOX(12, &received) )
		{
			df.PWMSet = received;
			df.KillCounter  = 200;
		}
		if( canDrv.ReadCANMBOX(13, &received) ) df.YawRef = received;


		// Run controller
		ctrlMdl.Ctrl_U.Mode = df.Mode;
		ctrlMdl.Ctrl_U.AckErrr = df.AckError;
		df.AckError = false; // clear ACK
		ctrlMdl.Ctrl_U.PWMSet = df.PWMSet;
		ctrlMdl.Ctrl_U.YawRef = df.YawRef;
		ctrlMdl.Ctrl_U.dYaw = df.dYaw;
		ctrlMdl.Ctrl_U.MagY = df.MagY;
		ctrlMdl.Ctrl_U.MagZ = df.MagZ;
		ctrlMdl.step();
		df.ActualMode = ctrlMdl.Ctrl_Y.ActualMode;
		df.Failed = ctrlMdl.Ctrl_Y.Failed;
		df.PWM1 = ctrlMdl.Ctrl_Y.PWM1;
		df.PWM2 = ctrlMdl.Ctrl_Y.PWM2;

		// Kill switch/watchdog code
		if( df.Failed ) df.KillCounter = 0; // kill
		if( df.KillCounter > 0 ) df.KillCounter--;
		else
		{
			// KILL
			df.PWM1 = 1000;
			df.PWM2 = 1000;
		}



		// Send Can Commands! (100 Hz)
		canDrv.SendMBOX(23, df.PWM1);
		canDrv.SendMBOX(24, df.PWM2);

		// send at 100Hz/100 = 10Hz
		if( (df.Counter % 10) == 0)
		{
			canDrv.SendMBOX(20, df.LoopTimeMS);
			canDrv.SendMBOX(21, df.MissionTime);
			canDrv.SendMBOX(22, df.LoopTimeMSMAX);

			canDrv.SendMBOX(25, (float)df.ActualMode);
			canDrv.SendMBOX(26, (float)df.Failed);
		}
		// send at 100Hz/100 = 1Hz
		if( (df.Counter % 100) == 0)
		{
			// send status
			/*
			usStatus.SetHealth(0);
			float battLife = (df.BattVoltage-20)*20;
			if( battLife > 100 ) battLife = 100;
			if( battLife < 0 ) battLife = 0;
			usStatus.SetBatteryLife((int)battLife);
			std::string sentence;
			usStatus.GenerateSentence(sentence);
			uartB.QueueData((char*)sentence.c_str());
			*/
		}



		// Blink LED
		if( (df.Counter % 20) == 0)
		{
			ledDebug.Toggle(DebugLED::GREEN);
		}

		//-----------------------------------------------------------
		// finish loop
		df.LoopTimeMS = timer.ElapsedTime() * 1000; // get looptime (ms)
		if(df.LoopTimeMSMAX < df.LoopTimeMS) df.LoopTimeMSMAX = df.LoopTimeMS;

		while( !timer.HasElapsed(STEPTIME) )
		{
			// do something...
		};
		df.MissionTime += timer.ElapsedTime();
	}
}

bool GetNextSentence(char* buffer, int count)
{
	bool exe = false;
	for(int i=0; i!=count; i++)
	{
		if( buffer[i] == '$' || g_GlobalSerIndex >= 200)
		{
			// restart
			g_GlobalSerIndex = 0;
		}

		g_GlobalSerBuffer[g_GlobalSerIndex] = buffer[i];
		g_GlobalSerIndex++;

		if( buffer[i] == '\r' )
		{
			exe = true;
			/*
			// parse and execute command
			if( urManual.VerifyDataType(g_GlobalSerBuffer) )
			{
				if( urManual.ParseSentence(g_GlobalSerBuffer) == 0 )
				{
					df.PWM1 = urManual.GetPWM1();
					df.PWM2 = urManual.GetPWM2();
					df.PWM3 = urManual.GetPWM3();
					df.Srv1 = urManual.GetSrv1();
					df.Srv2 = urManual.GetSrv2();
					df.KillCounter  = 200;
				}
			}
			else if( urLights.VerifyDataType(g_GlobalSerBuffer) )
			{
				if( urLights.ParseSentence(g_GlobalSerBuffer) == 0 )
				{
					df.Lights = urLights.GetLightsCode();
				}
			}
			*/
		}
	}

	return exe;
}

// SETUP BOXES
void CANSetupBoxes(CANDriver* ptr)
{
	// RX
	ptr->SetupMBOX(0,  IO_BASE+0x000311, 8, false); // Voltage
	ptr->SetupMBOX(1,  IMU_BASE+0x000302, 8, false); // dYaw(GyroZ)
	ptr->SetupMBOX(2,  IMU_BASE+0x000341, 8, false); // MagY
	ptr->SetupMBOX(3,  IMU_BASE+0x000342, 8, false); // MagZ

	// RX Commands!!!
	ptr->SetupMBOX(10,  CAN_BASE+0x000100, 8, false); // Mode (X)
	ptr->SetupMBOX(11,  CAN_BASE+0x000101, 8, false); // AckError (X)
	ptr->SetupMBOX(12,  CAN_BASE+0x000102, 8, false); // PWMSet (X)
	ptr->SetupMBOX(13,  CAN_BASE+0x000103, 8, false); // YawRef (X)

	// TX
	ptr->SetupMBOX(20,  CAN_BASE+0x000350, 8, true); // Loop time
	ptr->SetupMBOX(21,  CAN_BASE+0x000351, 8, true); // TotalTime
	ptr->SetupMBOX(22,  CAN_BASE+0x000352, 8, true); // LooptimeMAX

	ptr->SetupMBOX(23,  IO_BASE+0x000120, 8, true); // PWM1
	ptr->SetupMBOX(24,  IO_BASE+0x000121, 8, true); // PWM2
	ptr->SetupMBOX(25,  CAN_BASE+0x000400, 8, true); // Mode real (X)
	ptr->SetupMBOX(26,  CAN_BASE+0x000401, 8, true); // Fail (X)
}
